﻿// Creado por Sergio. 14/06/10
// Modificado por Sergio. 23-24-25/06/10. Testeadas nuevas funcionalidas y modificado para que funcione mejor

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Engine
{
    /// <summary>
    /// Clase que contiene informacion de la transformacion en el mundo: posicion y rotacion
    /// </summary>
    public class Transform
    {
        Matrix mTranslationMatrix; // matriz de traslacion
        Matrix mRotationMatrix; // matriz de rotacion
        Matrix mScaleMatrix; // matriz de escala

        Vector3 mPrevPosition = Vector3.Zero; // posicion previa (para colisiones)
        Vector3 mPosition = Vector3.Zero; // posicion
        Vector3 mEulerAngles = Vector3.Zero; // angulo en x, y, z (en degrees)
        Vector3 mScale = Vector3.One; // escala
        
        #region Constructores

        public Transform()
        {
            this.Scale = Vector3.One;
            this.EulerAngles = Vector3.Zero;
            this.Position = Vector3.Zero;
        }
        public Transform(Vector3 pos)
        {
            this.Scale = Vector3.One;
            this.EulerAngles = Vector3.Zero;
            this.Position = pos;
        }
        public Transform(Vector3 pos, Vector3 euler)
        {
            this.Scale = Vector3.One;
            this.EulerAngles = euler;
            this.Position = pos;
        }
        public Transform(Vector3 pos, Vector3 euler, Vector3 scale)
        {
            this.Scale = scale;
            this.EulerAngles = euler;
            this.Position = pos;
        }

        #endregion

        // Vectores de direccion locales, es decir, respecto de esta transformacion
        public Vector3 Forward { get { return mRotationMatrix.Forward; } }
        public Vector3 Backward { get { return mRotationMatrix.Backward; } }
        public Vector3 Right { get { return mRotationMatrix.Right; } }
        public Vector3 Left { get { return mRotationMatrix.Left; } }
        public Vector3 Up { get { return mRotationMatrix.Up; } }
        public Vector3 Down { get { return mRotationMatrix.Down; } }

        /// <summary>
        /// Set y Get de Angulos (Euler y Degrees)
        /// </summary>
        public Vector3 EulerAngles
        {
            get { return mEulerAngles; }
            set
            {
                mEulerAngles = value;
                mRotationMatrix = Matrix.CreateFromYawPitchRoll(MathHelper.ToRadians(mEulerAngles.Y),
                    MathHelper.ToRadians(mEulerAngles.X), MathHelper.ToRadians(mEulerAngles.Z));
            }
        }

        /// <summary>
        /// Set y Get de Escala
        /// </summary>
        public Vector3 Scale
        {
            get { return mScale; }
            set
            {
                mScale = value;
                mScaleMatrix = Matrix.CreateScale(mScale);
            }
        }

        /// <summary>
        /// Set y Get de Posicion
        /// </summary>
        public Vector3 Position
        {
            get { return mPosition; }
            set
            {
                mPosition = value;
                mTranslationMatrix = Matrix.CreateTranslation(mPosition);
            }
        }

        public Vector3 LastPosition
        {
            get { return this.mPrevPosition; }
            set { this.mPrevPosition = value; }
        }

        /// <summary>
        /// Get de la matriz de mundo (o sea, la de escala, rotacion y traslacion multiplicadas)
        /// </summary>
        public Matrix WorldMatrix
        {
            get { return (mScaleMatrix * mRotationMatrix * mTranslationMatrix); }
        }
       

        public override string ToString()
        {
            return ("Position: " + Position.ToString() + "\n" +
                    "EulerAngles: " + EulerAngles.ToString() + "\n" +
                    "Scale: " + Scale.ToString());
        }

        /// <summary>
        /// Setea la rotacion para que mire al punto recibido por parametro
        /// </summary>
        /// <param name="target"> Objetivo a mirar </param>
        public void LookAt(Vector3 target)
        {
            Matrix rotation = Matrix.Identity;

            rotation.Forward = Vector3.Normalize(target - mPosition);
            rotation.Right = Vector3.Normalize(Vector3.Cross(rotation.Forward, Vector3.Up));
            rotation.Up = Vector3.Normalize(Vector3.Cross(rotation.Right, rotation.Forward));

            Vector3 sca = Vector3.Zero;
            Vector3 pos = Vector3.Zero;
            Quaternion rot = Quaternion.Identity;
            rotation.Decompose(out sca, out rot, out pos);

            Vector3 euler = QuaternionToEuler(rot);

            euler = new Vector3(MathHelper.ToDegrees(euler.Z), MathHelper.ToDegrees(euler.X), MathHelper.ToDegrees(euler.Y));
            EulerAngles = euler;

            //this.mRotationMatrix = rotation;
        }

        /// <summary>
        /// Transforma una rotacion de quaternion a euler // lo entrega yaw roll pitch
        /// </summary>
        /// <param name="q"> Quaternion a convertir </param>
        /// <returns> Vector3 en angulos Euler convertido </returns>
        Vector3 QuaternionToEuler(Quaternion q) // lo entrega yaw roll pitch
        {
            Vector3 v = new Vector3();

            v.X = (float)Math.Atan2
            (
                2 * q.Y * q.W - 2 * q.X * q.Z,
                1 - 2 * Math.Pow(q.Y, 2) - 2 * Math.Pow(q.Z, 2)
            );

            v.Y = (float)Math.Asin
            (
                2 * q.X * q.Y + 2 * q.Z * q.W
            );

            v.Z = (float)Math.Atan2
            (
                2 * q.X * q.W - 2 * q.Y * q.Z,
                1 - 2 * Math.Pow(q.X, 2) - 2 * Math.Pow(q.Z, 2)
            );

            if (q.X * q.Y + q.Z * q.W == 0.5)
            {
                v.X = (float)(2 * Math.Atan2(q.X, q.W));
                v.Z = 0;
            }

            else if (q.X * q.Y + q.Z * q.W == -0.5)
            {
                v.X = (float)(-2 * Math.Atan2(q.X, q.W));
                v.Z = 0;
            }

            return v; // lo entrega yaw roll pitch
        }

        /// <summary>
        /// Traslada posicion en alguna direccion
        /// </summary>
        /// <param name="translation"> direccion a la que traslada </param>
        public void Translate(Vector3 translation)
        {
            this.Position += translation;
        }

        /// <summary>
        /// Rota sobre si mismo
        /// </summary>
        /// <param name="axis"> eje sobre el que rota </param>
        /// <param name="angle"> angulo sobre el que rota </param>
        public void Rotate(Vector3 axis, float angle)
        {
            this.EulerAngles += axis * angle;
        }

        /// <summary>
        /// Rota posicion alrededor de un punto
        /// </summary>
        /// <param name="originPoint"> punto alrededor del cual rota </param>
        /// <param name="rotationAxis"> eje sobre el cual rota </param>
        /// <param name="radiansToRotate"> cantidad de radianes que rota </param>
        public void RotateAround(Vector3 originPoint, Vector3 rotationAxis, float radiansToRotate)
        {
            Vector3 diffVect = mPosition - originPoint;

            Vector3 rotatedVect = Vector3.Transform(diffVect, Matrix.CreateFromAxisAngle(rotationAxis, radiansToRotate));

            rotatedVect += originPoint;

            this.Position = rotatedVect;
        }

    }
}